
function [c, ceq, Gc, Gceq] = Bcr4bp_ConsEMRot2D_Coe(vars , aux)
%
% 构造约束和梯度
% 1) 2D
% 2) EMRot
%
% 作者：张晨
% 单位：中科院空间应用工程与技术中心
% 时间：2022年02月22日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c = [];
ceq = [];
Gc = [];
Gceq = [];

% ------------------------------------------- 提取变量 ------------------------------------------
% 变量数
nVar = size(vars , 1);

% 节点数
nNode = aux.nNode;

% 状态矩阵
var_mtx = reshape(vars(1 : 4 * nNode) , 4 , nNode); % 状态

% 开始时间
tau_1 = vars(4 * nNode + 1);

% 结束时间
tau_f = vars(4 * nNode + 2);

% 时间向量
tau_vec = linspace(tau_1 , tau_f , nNode)';

% -------------------------------------- 输出预分配 -------------------------------------
% 无不等式约束
c = [];
Gc = [];

% 状态连接约束
stateCeq = zeros(4 * (nNode - 1) , 1);
stateGceq = zeros(4 * (nNode - 1) , nVar);
stateCeqCount = 0;

% 保存每段时间和状态
seg_tt_all = [];
seg_xx_all = [];

% 构造多步打靶约束
for jLoop = 1 : (nNode - 1)

    % 提取单轨道段两端的时刻
    tau_k = tau_vec(jLoop);
    tau_kp1 = tau_vec(jLoop + 1);

    % 提取单轨道段两端的打靶变量
    var_tauk = var_mtx(: , jLoop);
    var_taukp1 = var_mtx(: , jLoop + 1);

    % 提取单轨道段两端的状态变量
    if jLoop == 1

        xx_tauk = Bcr4bp_var2xxEMRot2D(var_mtx(: , 1) , 'leo' , aux);
        xx_tauk = xx_tauk';
        xx_taukp1 = var_mtx(: , jLoop + 1);

    elseif jLoop == (nNode - 1)

        xx_tauk = var_mtx(: , jLoop);
        xx_taukp1 = Bcr4bp_var2xxEMRot2D(var_mtx(: , nNode) , 'llo' , aux);
        xx_taukp1 = xx_taukp1';

    else

        xx_tauk = var_mtx(: , jLoop);
        xx_taukp1 = var_mtx(: , jLoop + 1);

    end

    % 数值积分
    phi0 = eye(4);
    options = odeset('Reltol' , aux.tol , 'AbsTol' , aux.tol);
    [seg_tt , seg_xx] = ode113(@Bcr4bp_StmEMRot2D , [tau_k , tau_kp1] , [xx_tauk ; phi0(:)] , options , aux);

    %     % 【测试】
    %     plotOrb2D(seg_xx , 'r' , 3);

    % 保存全部轨道
    seg_tt_all = [seg_tt_all ; seg_tt];
    seg_xx_all = [seg_xx_all ; seg_xx];

    % 积分终端状态
    xx_taukp1_ = seg_xx(end , 1 : 4)';

    % 状态转移矩阵
    phi_tauk_taukp1 = reshape(seg_xx(end , 5 : 20) , 4 , 4);

    % 状态连接约束和梯度
    [stateCeqN_temp , stateCeq_temp , stateGceq_temp] = Bcr4bp_StateConsEMRot2D_Coe(tau_k , xx_tauk , var_tauk , ...
        tau_kp1 , xx_taukp1 , var_taukp1 , ...
        xx_taukp1_ , phi_tauk_taukp1 , nVar , nNode , jLoop , aux);

    % 更新
    stateCeq(stateCeqCount + 1 : stateCeqCount + stateCeqN_temp , :) = stateCeq_temp;
    stateGceq(stateCeqCount + 1 : stateCeqCount + stateCeqN_temp , :) = stateGceq_temp;
    stateCeqCount = stateCeqCount + stateCeqN_temp;

end

%% ---------------------------------------------- dep 约束 ------------------------------------------
mu = aux.EMRot.mu;

rMag1 = var_mtx(1 , 1);
alpha1 = var_mtx(2 , 1);
beta1 = var_mtx(3 , 1);
temp1 = var_mtx(4 , 1);

% LEO约束
depCeq = [rMag1 - aux.dim.rMagLEO / aux.dim.EMRot_l;
    temp1];

% LEO约束梯度
depGceq = zeros(2 , nVar);
depGceq(1 , 1) = 1;
depGceq(2 , 4) = 1;

%% ---------------------------------------------- arr 约束 ------------------------------------------
rMagn = var_mtx(1 , end);
alphan = var_mtx(2 , end);
betan = var_mtx(3 , end);
tempn = var_mtx(4 , end);

% LLO约束
arrCeq = [rMagn - aux.dim.rMagLLO / aux.dim.EMRot_l;
    tempn];

% LLO约束梯度
arrGceq = zeros(2 , nVar);
arrGceq(1 , 4 * (nNode - 1) + 1) = 1;
arrGceq(2 , 4 * (nNode - 1) + 4) = 1;

%% 拼约束和梯度
% 约束拼接
ceq = [stateCeq ; depCeq ; arrCeq];

% 梯度拼接
Gceq = [stateGceq ; depGceq ; arrGceq];

%% --------------------------------------------------- 伪弧长延拓 ---------------------------------------------------
% 1）约束总飞行时间, tof
% 2）约束出发脉冲, dv_t0
% 3）约束到达脉冲, dv_tf
% 4）约束ThetaS(t0), thetaS_t0
% 5）约束ThetaS(tf), thetaS_tf

% %% ---------------------------------------------- dep 约束 ------------------------------------------


try

    pseudoArc = aux.pseudoArc;

    if strcmp(pseudoArc.IO , 'on')

        if strcmp(pseudoArc.type , 'tof')

            ceq = [ceq ; (tau_f - tau_1) - pseudoArc.desiredValue];
            Gceq_temp = zeros(1 , nVar);
            Gceq_temp(4 * nNode + 1) = -1;
            Gceq_temp(4 * nNode + 2) = 1;
            Gceq = [Gceq; Gceq_temp];

        elseif strcmp(pseudoArc.type , 'dv_t0')

            ceq = [ceq ; beta1 - pseudoArc.desiredValue];
            Gceq_temp = zeros(1 , nVar);
            Gceq_temp(3) = 1;
            Gceq = [Gceq; Gceq_temp];

        elseif strcmp(pseudoArc.type , 'dv_tf')

            ceq = [ceq ; betan - pseudoArc.desiredValue];
            Gceq_temp = zeros(1 , nVar);
            Gceq_temp(4 * (nNode - 1) + 3) = 1;
            Gceq = [Gceq; Gceq_temp];

        elseif strcmp(pseudoArc.type , 'thetaS_t0')

            ceq = [ceq ; aux.EMRot.ws * tau_1 - pseudoArc.desiredValue];
            Gceq_temp = zeros(1 , nVar);
            Gceq_temp(4 * nNode + 1) = aux.EMRot.ws;
            Gceq = [Gceq; Gceq_temp];

        elseif strcmp(pseudoArc.type , 'thetaS_tf')

            ceq = [ceq ; aux.EMRot.ws * tau_f - pseudoArc.desiredValue];
            Gceq_temp = zeros(1 , nVar);
            Gceq_temp(4 * nNode + 2) = aux.EMRot.ws;
            Gceq = [Gceq; Gceq_temp];

        else

            fprintf('pseudoArc.type 类型错误！无法识别！')

        end

    end

catch

    dd = [];

end

% 构造约束和梯度
Gceq = Gceq';
Gc = Gc';

end


function [ceqNum , ceq , Gceq] = Bcr4bp_StateConsEMRot2D_Coe(tau_k , xx_tauk , var_tauk , ...
    tau_kp1 , xx_taukp1 , var_taukp1 , ...
    xx_taukp1_ , phi_tauk_taukp1 , nVar , nNode , jLoop , aux)
%
% 构造状态连接约束
%
% 注意：
% 如果xxkp1是'dv'，则约束为r1f - r20
% 否则都为x1f - x20
%
% 作者：张晨
% 邮箱：chenzhang@csu.ac.cn
% 单位：中国科学院空间应用工程与技术中心，空间探索室
% 时间：2021年08月25日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ---------------------------------------------------------------------------
% tauk动力学
dxx_tauk = Bcr4bp_EqmEMRot2D(tau_k , xx_tauk , aux);

% taukp1_动力学
dxx_taukp1_ = Bcr4bp_EqmEMRot2D(tau_kp1 , xx_taukp1_ , aux);

% 模块
dtauj_dtau1 = 1 - (jLoop - 1) / (nNode - 1);
dtaujp1_dtau1 = 1 - jLoop / (nNode - 1);
dtauj_dtaun = (jLoop - 1) / (nNode - 1);
dtaujp1_dtaun = jLoop / (nNode - 1);

% ---------------------------------------------------------------------------
% 连接约束数
ceqNum = 4;

% 约束
ceq = (xx_taukp1_ - xx_taukp1);

% 梯度预分配
Gceq = zeros(4 , nVar);

% --------------- 状态连接约束（梯度） -------------
% wrt xx_tauk
if jLoop == 1

    Gceq(: , 4 * (jLoop - 1) + 1 : 4 * jLoop) = phi_tauk_taukp1  * dxde_leo(var_tauk , aux);

else

    Gceq(: , 4 * (jLoop - 1) + 1 : 4 * jLoop) = phi_tauk_taukp1;

end

% wrt xxkp1
if jLoop == (nNode - 1)

    Gceq(: , 4 * jLoop + 1 : 4 * (jLoop + 1)) = - eye(4) * dxde_llo(var_taukp1 , aux);

else

    Gceq(: , 4 * jLoop + 1 : 4 * (jLoop + 1)) = - eye(4);

end

% 状态连接约束 wrt tau_1
Gceq(: , 4 * nNode + 1) = - phi_tauk_taukp1 * dxx_tauk * dtauj_dtau1 + dxx_taukp1_ * dtaujp1_dtau1;

% 状态连接约束 wrt tau_n
Gceq(: , 4 * nNode + 2) = - phi_tauk_taukp1 * dxx_tauk * dtauj_dtaun + dxx_taukp1_ * dtaujp1_dtaun;

end
